#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "3d_math.h"

#ifndef M_PI //理论上在 math.h 中有定义
#define M_PI 3.14159265358979323846
#endif

/*
 *  quaternion解算
 *  参数:
 *      quat_err[7]: 四元数和误差积累数组,初始值用 {1,0,0,0,0,0,0} (必要参数)
 *      valG[3]: 陀螺仪xyz轴输出,单位:deg/s (必要参数)
 *      valA[3]: 加速度xyz轴输出,单位:g  (可以置NULL,等于纯陀螺仪计算姿态)
 *      pry[3]: 输出绕xyz轴角度,单位:rad (可以置NULL)
 *      gravity[3]: 返回重力向量 (可以置NULL)
 *      intervalMs: 采样间隔,单位:ms (必要参数)
 */
void quat_pry(float quat_err[7], float valG[3], float valA[3], float pry[3], float gravity[3], int32_t intervalMs)
{
    float Kp = 2.0f;
    float Ki = 0.001f;
    // 四元数的元素，代表估计方向
    // static float qBak[4] = {1.0f, 0.0f, 0.0f, 0.0f};
    // 按比例缩小积分误差
    // static float eIntBak[3] = {0.0f, 0.0f, 0.0f};
    // 时间间隔一半, 后面 pi/180 用于 deg/s 转 rad/s
    float halfT = (float)intervalMs / 2 / 1000;
    float q[4];
    float eInt[3];
    float norm;
    float ax, ay, az;
    float gx, gy, gz;
    float vx, vy, vz;
    float ex, ey, ez;
    if (!valG)
        return;
    // stack out
    // memcpy(q, qBak, sizeof(float) * 4);
    // memcpy(eInt, eIntBak, sizeof(float) * 3);
    memcpy(q, &quat_err[0], sizeof(float) * 4);
    memcpy(eInt, &quat_err[4], sizeof(float) * 3);
    // 估计重力的方向(vx, vy, vz)
    vx = ax = 2 * (q[1] * q[3] - q[0] * q[2]);
    vy = ay = 2 * (q[0] * q[1] + q[2] * q[3]);
    vz = az = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    if (gravity)
    {
        gravity[0] = vx;
        gravity[1] = vy;
        gravity[2] = vz;
    }
    // 加速度向量转为单位向量
    if (valA)
    {
        norm = sqrt(valA[0] * valA[0] + valA[1] * valA[1] + valA[2] * valA[2]);
        if (!isnan(norm))
        {
            ax = valA[0] / norm;
            ay = valA[1] / norm;
            az = valA[2] / norm;
            // 动态参数,当重力失真(自由落体/超重)时减少对加速度计依赖
            if (norm <= 0.99f && norm > 0.79f)
            {
                norm = pow(norm - 0.79f, 1) / 0.2f;
                Kp *= norm;
                Ki *= norm;
            }
            else if (norm > 0.99 && norm < 1.19f)
            {
                norm = pow(1.19f - norm, 1) / 0.2f;
                Kp *= norm;
                Ki *= norm;
            }
            else
                Kp = Ki = 0.0f;
        }
    }
    // 叉积补偿滤波(互补滤波) https://blog.csdn.net/weixin_40378598/article/details/108133032
    // 加速度向量(ax, ay, az)和向量(vx, vz, vy)的叉乘, gxyz 和 vxyz 夹角越大(90度时最大)则 exyz 值越大
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);
    // 积分误差比例积分增益
    eInt[0] += ex * Ki;
    eInt[1] += ey * Ki;
    eInt[2] += ez * Ki;
    // 调整后的陀螺仪测量
    gx = valG[0] * M_PI / 180 + Kp * ex + eInt[0];
    gy = valG[1] * M_PI / 180 + Kp * ey + eInt[1];
    gz = valG[2] * M_PI / 180 + Kp * ez + eInt[2];
    // 四元数微分方程
    q[0] += (-q[1] * gx - q[2] * gy - q[3] * gz) * halfT;
    q[1] += (q[0] * gx + q[2] * gz - q[3] * gy) * halfT;
    q[2] += (q[0] * gy - q[1] * gz + q[3] * gx) * halfT;
    q[3] += (q[0] * gz + q[1] * gy - q[2] * gx) * halfT;
    // 单位化
    norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    if (isnan(norm))
    {
        // printf(" quat_pry: nan \r\n");
        return;
    }
    q[0] /= norm;
    q[1] /= norm;
    q[2] /= norm;
    q[3] /= norm;
    // pry
    if (pry)
    {
        pry[1] = asin(-2 * q[1] * q[3] + 2 * q[0] * q[2]);
        pry[0] = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1);
        pry[2] = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
    }
    // stack in
    // memcpy(qBak, q, sizeof(float) * 4);
    // memcpy(eIntBak, eInt, sizeof(float) * 3);
    memcpy(&quat_err[0], q, sizeof(float) * 4);
    memcpy(&quat_err[4], eInt, sizeof(float) * 3);
}

// 四元数角增量(龙格塔微分方程)
void quat_diff(float q[4], float roll_xyz[3])
{
    float norm;
#if 1
    //一次增量(精度较差)
    q[0] += (-q[1] * roll_xyz[0] - q[2] * roll_xyz[1] - q[3] * roll_xyz[2]) / 2;
    q[1] += (q[0] * roll_xyz[0] + q[2] * roll_xyz[2] - q[3] * roll_xyz[1]) / 2;
    q[2] += (q[0] * roll_xyz[1] - q[1] * roll_xyz[2] + q[3] * roll_xyz[0]) / 2;
    q[3] += (q[0] * roll_xyz[2] + q[1] * roll_xyz[1] - q[2] * roll_xyz[0]) / 2;
#else
    //分多次增量(精度较好)
    float maxValue;
    int32_t i, div;

    if (fabs(roll_xyz[0]) > fabs(roll_xyz[1]))
        maxValue = fabs(roll_xyz[0]);
    else
        maxValue = fabs(roll_xyz[1]);
    if (fabs(roll_xyz[2]) > maxValue)
        maxValue = fabs(roll_xyz[2]);

    div = (int)(maxValue * 50) + 1;
    roll_xyz[0] /= div;
    roll_xyz[1] /= div;
    roll_xyz[2] /= div;

    for (i = 0; i < div; i++) {
        q[0] += (-q[1] * roll_xyz[0] - q[2] * roll_xyz[1] - q[3] * roll_xyz[2]) / 2;
        q[1] += (q[0] * roll_xyz[0] + q[2] * roll_xyz[2] - q[3] * roll_xyz[1]) / 2;
        q[2] += (q[0] * roll_xyz[1] - q[1] * roll_xyz[2] + q[3] * roll_xyz[0]) / 2;
        q[3] += (q[0] * roll_xyz[2] + q[1] * roll_xyz[1] - q[2] * roll_xyz[0]) / 2;
    }
#endif
    // 单位化
    norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    if (!isnan(norm))
    {
        q[0] /= norm;
        q[1] /= norm;
        q[2] /= norm;
        q[3] /= norm;
    }
}
// roll_xyz使用单位: 度
void quat_diff2(float q[4], float roll_xyz[3])
{
    float _roll_xyz[3];
    _roll_xyz[0] = roll_xyz[0] * M_PI / 180;
    _roll_xyz[1] = roll_xyz[1] * M_PI / 180;
    _roll_xyz[2] = roll_xyz[2] * M_PI / 180;
    quat_diff(q, _roll_xyz);
}

// 四元数乘法
void quat_multiply(float q1[4], float q2[4], float ret[4])
{
    float _ret[4];
    _ret[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
    _ret[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
    _ret[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
    _ret[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
    ret[0] = _ret[0];
    ret[1] = _ret[1];
    ret[2] = _ret[2];
    ret[3] = _ret[3];
}

// 欧拉角转四元数(zyx顺序)
void pry_to_quat(float pry[3], float q[4])
{
    float Qp[4] = {0};
    float Qr[4] = {0};
    float Qy[4] = {0};
    float norm;

    Qp[0] = cos(pry[0] / 2);
    Qp[1] = sin(pry[0] / 2);

    Qr[0] = cos(pry[1] / 2);
    Qr[2] = sin(pry[1] / 2);

    Qy[0] = cos(pry[2] / 2);
    Qy[3] = sin(pry[2] / 2);

    quat_multiply(Qy, Qr, q);
    quat_multiply(q, Qp, q);
    // 单位化
    norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    if (!isnan(norm))
    {
        q[0] /= norm;
        q[1] /= norm;
        q[2] /= norm;
        q[3] /= norm;
    }
}
// pry使用单位: 度
void pry_to_quat2(float pry[3], float q[4])
{
    float _pry[3];
    _pry[0] = pry[0] * M_PI / 180;
    _pry[1] = pry[1] * M_PI / 180;
    _pry[2] = pry[2] * M_PI / 180;
    pry_to_quat(_pry, q);
}

// 四元数转欧拉角
void quat_to_pry(float q[4], float pry[3])
{
    pry[1] = asin(-2 * q[1] * q[3] + 2 * q[0] * q[2]);
    pry[0] = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1);
    pry[2] = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
}
// pry使用单位: 度
void quat_to_pry2(float q[4], float pry[3])
{
    quat_to_pry(q, pry);
    pry[0] *= 180 / M_PI;
    pry[1] *= 180 / M_PI;
    pry[2] *= 180 / M_PI;
}

/*
 *  四元数方式旋转和逆旋转
 *  参数:
 *      quat[4]: 使用已有的四元数(可置NULL), 将不使用 xyzSrc 和 rad
 *      xyzSrc[3]: 要绕转的空间向量,右手旋转,大拇指向量方向
 *      rad: 旋转角度,单位:rad
 *      xyzRet[3]: 被旋转的向量,输出结果覆写到此
 *      T: 转置
 */
void quat_roll_array(float quat[4], float xyzSrc[3], float rad, float *xyzRet, int32_t len, bool T)
{
    float q[4], qT[4];
    float rv[3];
    float v[4], ret[4];
    float norm;
    int32_t i;

    if (quat)
    {
        memcpy(q, quat, sizeof(float) * 4);
        // 单位化
        norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
        if (!isnan(norm))
        {
            q[0] /= norm;
            q[1] /= norm;
            q[2] /= norm;
            q[3] /= norm;
        }
    }
    else
    {
        //对旋转轴进行单位向量处理(否则旋转后会附带缩放效果)
        memcpy(rv, xyzSrc, sizeof(float) * 3);
        //单位化
        norm = sqrt(rv[0] * rv[0] + rv[1] * rv[1] + rv[2] * rv[2]);
        if (!isnan(norm))
        {
            rv[0] /= norm;
            rv[1] /= norm;
            rv[2] /= norm;
        }
        //
        q[0] = cos(rad / 2);
        q[1] = sin(rad / 2) * rv[0];
        q[2] = sin(rad / 2) * rv[1];
        q[3] = sin(rad / 2) * rv[2];
    }

    qT[0] = q[0];
    qT[1] = -q[1];
    qT[2] = -q[2];
    qT[3] = -q[3];

    for (i = 0; i < len * 3; i += 3)
    {
        v[0] = 0;
        v[1] = xyzRet[i];
        v[2] = xyzRet[i + 1];
        v[3] = xyzRet[i + 2];

        if (T)
        {
            quat_multiply(qT, v, ret);
            quat_multiply(ret, q, ret);
        }
        else
        {
            quat_multiply(q, v, ret);
            quat_multiply(ret, qT, ret);
        }

        memcpy(&xyzRet[i], &ret[1], sizeof(float) * 3);
    }
}
void quat_roll(float quat[4], float xyzSrc[3], float rad, float xyzRet[3], bool T)
{
    quat_roll_array(quat, xyzSrc, rad, xyzRet, 1, T);
}

static void _quat_roll_xyz(float roll_xyz[3], float xyz[3], float retXyz[3], bool zyx)
{
    float qx[4] = {0}, qy[4] = {0}, qz[4] = {0};
    float qxT[4] = {0}, qyT[4] = {0}, qzT[4] = {0};
    float v[4], ret[4];

    qx[0] = qxT[0] = cos(roll_xyz[0] / 2);
    qx[1] = sin(roll_xyz[0] / 2);
    qxT[1] = -qx[1];

    qy[0] = qyT[0] = cos(roll_xyz[1] / 2);
    qy[2] = sin(roll_xyz[1] / 2);
    qyT[2] = -qy[2];

    qz[0] = qzT[0] = cos(roll_xyz[2] / 2);
    qz[3] = sin(roll_xyz[2] / 2);
    qzT[3] = -qz[3];

    v[0] = 0;
    v[1] = xyz[0];
    v[2] = xyz[1];
    v[3] = xyz[2];

    if (zyx)
    {
        quat_multiply(qz, qy, ret);
        quat_multiply(ret, qx, ret);
        quat_multiply(ret, v, ret);
        quat_multiply(ret, qxT, ret);
        quat_multiply(ret, qyT, ret);
        quat_multiply(ret, qzT, ret);

        // quat_multiply(qxT, qyT, ret);
        // quat_multiply(ret, qzT, ret);
        // quat_multiply(ret, v, ret);
        // quat_multiply(ret, qz, ret);
        // quat_multiply(ret, qy, ret);
        // quat_multiply(ret, qx, ret);
    }
    else
    {
        quat_multiply(qx, qy, ret);
        quat_multiply(ret, qz, ret);
        quat_multiply(ret, v, ret);
        quat_multiply(ret, qzT, ret);
        quat_multiply(ret, qyT, ret);
        quat_multiply(ret, qxT, ret);
    }

    memcpy(retXyz, &ret[1], sizeof(float) * 3);
}

/*
 *  四元数依次三轴旋转
 *  参数:
 *      roll_xyz: 绕三轴旋转,单位:rad
 *      xyz: 目标点
 *      retXyz: 旋转和平移后结果写到此
 */
void quat_xyz(float roll_xyz[3], float xyz[3], float retXyz[3])
{
    _quat_roll_xyz(roll_xyz, xyz, retXyz, false);
}
void quat_zyx(float roll_xyz[3], float xyz[3], float retXyz[3])
{
    _quat_roll_xyz(roll_xyz, xyz, retXyz, true);
}


//获取平面三角形最长边
int32_t triangle_max_line(int32_t xy[6])
{
    float L1 = (xy[0] - xy[2]) * (xy[0] - xy[2]) + (xy[1] - xy[3]) * (xy[1] - xy[3]);
    float L2 = (xy[0] - xy[4]) * (xy[0] - xy[4]) + (xy[1] - xy[5]) * (xy[1] - xy[5]);
    float L3 = (xy[2] - xy[4]) * (xy[2] - xy[4]) + (xy[5] - xy[3]) * (xy[5] - xy[3]);
    if (L2 > L1)
    {
        if (L2 > L3)
            return (int)sqrt(L2);
        else
            return (int)sqrt(L3);
    }
    else
    {
        if (L1 > L3)
            return (int)sqrt(L1);
        else
            return (int)sqrt(L3);
    }
}
int32_t triangle_max_line3D(int32_t xy[9])
{
    float L1 = (xy[0] - xy[3]) * (xy[0] - xy[3]) + (xy[1] - xy[4]) * (xy[1] - xy[4]) + (xy[2] - xy[5]) * (xy[2] - xy[5]);
    float L2 = (xy[0] - xy[6]) * (xy[0] - xy[6]) + (xy[1] - xy[7]) * (xy[1] - xy[7]) + (xy[2] - xy[8]) * (xy[2] - xy[8]);
    float L3 = (xy[3] - xy[6]) * (xy[3] - xy[6]) + (xy[4] - xy[7]) * (xy[4] - xy[7]) + (xy[5] - xy[8]) * (xy[5] - xy[8]);
    if (L2 > L1)
    {
        if (L2 > L3)
            return (int)sqrt(L2);
        else
            return (int)sqrt(L3);
    }
    else
    {
        if (L1 > L3)
            return (int)sqrt(L1);
        else
            return (int)sqrt(L3);
    }
}

/*
 *  遍历平面三角形里面的每一个点
 *  参数:
 *      xy[6]: 3个二维坐标
 *      retXy: 返回二维坐标数组指针 !! 用完记得释放 !!
 *
 *  返回: retXy数组里的坐标个数
 */
int32_t triangle_enum(int32_t xy[6], int32_t **retXy)
{
    int32_t cMax, c = 0;
    int32_t i, j, k;
    int32_t maxLine = triangle_max_line(xy);

    //返回点个数估算
    cMax = maxLine + 1;

    //1+2+3+4+5...+100=? 等差数列求和问题
    cMax = cMax * (cMax + 1) / 2;

    //数组内存分配
    cMax *= 2;
    *retXy = (int32_t *)calloc(cMax, sizeof(int));

    //对于三角形ABC内一点P,有 P = i*A + j*B + k*C, 且 i + j + k = 1
    //即遍历所有的i,j,k数值即可获得所有P点
    for (i = 0; i < maxLine; i += 1)
    {
        for (j = 0; j < maxLine - i && c < cMax; j += 1)
        {
            k = maxLine - i - j;
            (*retXy)[c++] = (i * xy[0] + j * xy[2] + k * xy[4]) / maxLine;
            (*retXy)[c++] = (i * xy[1] + j * xy[3] + k * xy[5]) / maxLine;
        }
    }

    return c / 2;
}

//三维版本
int32_t triangle_enum3D(int32_t xyz[9], int32_t **retXyz)
{
    int32_t cMax, c = 0;
    int32_t i, j, k;
    int32_t maxLine = triangle_max_line3D(xyz);

    //返回点个数估算
    cMax = maxLine + 1;

    //1+2+3+4+5...+100=? 等差数列求和问题
    cMax = cMax * (cMax + 1) / 2;

    //数组内存分配
    cMax *= 3;
    *retXyz = (int32_t *)calloc(cMax, sizeof(int));

    //对于三角形ABC内一点P,有 P = i*A + j*B + k*C, 且 i + j + k = 1
    //即遍历所有的i,j,k数值即可获得所有P点
    for (i = 0; i < maxLine; i += 1)
    {
        for (j = 0; j < maxLine - i && c < cMax; j += 1)
        {
            k = maxLine - i - j;
            (*retXyz)[c++] = (i * xyz[0] + j * xyz[3] + k * xyz[6]) / maxLine;
            (*retXyz)[c++] = (i * xyz[1] + j * xyz[4] + k * xyz[7]) / maxLine;
            (*retXyz)[c++] = (i * xyz[2] + j * xyz[5] + k * xyz[8]) / maxLine;
        }
    }

    return c / 3;
}

static int32_t _abs(int32_t f)
{
    return f < 0 ? (-f) : f;
}

/*
 *  遍历平面直线所有的点
 *  参数:
 *      xy[4]: 2个二维坐标
 *      retXy: 返回二维坐标数组指针 !! 用完记得释放 !!
 *
 *  返回: retXy数组里的坐标个数
 */
int32_t line_enum(int32_t xy[4], int32_t **retXy)
{
    int32_t xErr, yErr;
    float xDiv, yDiv;
    float x, y;
    int32_t maxLine;
    int32_t cMax, c = 0;

    //xy偏差
    xErr = _abs(xy[2] - xy[0]);
    yErr = _abs(xy[3] - xy[1]);

    //最长边选取
    if (xErr > yErr)
        maxLine = xErr;
    else
        maxLine = yErr;

    cMax = maxLine + 1;

    //内存准备
    cMax *= 2;
    *retXy = (int32_t *)calloc(cMax, sizeof(int));

    //每次偏移量(水平、垂直时为0)
    xDiv = xErr == 0 ? 0 : ((xy[2] - xy[0]) / (float)maxLine); //注意这里必须用[2]减[0]
    yDiv = yErr == 0 ? 0 : ((xy[3] - xy[1]) / (float)maxLine); //同上

    for (x = xy[0], y = xy[1]; c < cMax; x += xDiv, y += yDiv)
    {
        (*retXy)[c++] = (int)x;
        (*retXy)[c++] = (int)y;
    }

    return c / 2;
}

//三维版本
int32_t line_enum3D(int32_t xyz[6], int32_t **retXyz)
{
    int32_t xErr, yErr, zErr;
    float xDiv, yDiv, zDiv;
    float x, y, z;
    int32_t maxLine;
    int32_t cMax, c = 0;

    //xy偏差
    xErr = _abs(xyz[3] - xyz[0]);
    yErr = _abs(xyz[4] - xyz[1]);
    zErr = _abs(xyz[5] - xyz[2]);

    //最长边选取
    if (xErr > yErr)
    {
        if (xErr > zErr)
            maxLine = xErr;
        else
            maxLine = zErr;
    }
    else
    {
        if (yErr > zErr)
            maxLine = yErr;
        else
            maxLine = zErr;
    }

    cMax = maxLine + 1;

    //内存准备
    cMax *= 3;
    *retXyz = (int32_t *)calloc(cMax, sizeof(int));

    //每次偏移量(水平、垂直时为0)
    xDiv = xErr == 0 ? 0 : ((xyz[3] - xyz[0]) / (float)maxLine); //注意这里必须用[3]减[0]
    yDiv = yErr == 0 ? 0 : ((xyz[4] - xyz[1]) / (float)maxLine); //同上
    zDiv = zErr == 0 ? 0 : ((xyz[5] - xyz[2]) / (float)maxLine); //同上

    for (x = xyz[0], y = xyz[1], z = xyz[2]; c < cMax; x += xDiv, y += yDiv, z += zDiv)
    {
        (*retXyz)[c++] = (int)x;
        (*retXyz)[c++] = (int)y;
        (*retXyz)[c++] = (int)z;
    }

    return c / 3;
}
